/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "commrobotiq.h"
#include <QDebug>
#include <QThread>

// ======== read

///===  register byte mapping =====================
///------byte------|---------write-----------|-----------read------------------
///  -----------------|--------0x03E8---------|-----------0x07D0-----------------------
///  -----0----------|----------激活-----------|----------激活-状态------------
/// ------1----------|---------------------------|--------------------------------
///  -----2----------|---------------------------|-----------fault --------------
///  -----3----------|---------pos-------------|-----------pos state----------
///  -----4----------|---------speed----------|-----------pos------------------
///  ------5---------|---------torque---------|-----------current-------------
///  ------6---------|---------------------------|-------------------------------


///=output register ===== write
///== byte0
//rACT = 1 = on = enable operation (always stay 1)
#define D_ROBOTIQ_rACT  0x01
//rGTO  = 1= go to positon    0=stop
#define D_ROBOTIQ_rGTO 0x08
// 1 = over writ all cmd and release and send fault
#define D_ROBOTIQ_rATR  0x10
//1 = auto-release
#define D_ROBOTIQ_rARD 0x20
///byte3  = pos   :   (open) 0x0～0xFF (close)
#define D_ROBOTIQ_rPR  0xFF
///byte4 = speed : 0xFF = max
#define D_ROBOTIQ_rSP  0xFF
///byte5 = torque : 0xFF = max
#define D_ROBOTIQ_rFR  0xFF


///==input registers & status====== read
///==byet 0
//1=actived
#define D_ROBOTIQ_gACT 0x01
//1= moving
#define D_ROBOTIQ_gGTO 0x08
//0x03=arivated  , 0x01=run
#define D_ROBOTIQ_gSTA 0x30
//object detection,  0x0=moving and no obj  , 0x01=open get, 0x02=closing get, 0x03= stop and no obj
#define D_ROBOTIQ_gOBJ 0xc0

///==byte2
//0x05 =上一个位置未完成就执行新位置了
//0x07 = not active
//0x0a  = 电压不足 ,    rACT needed
//0x0d = 干扰，   ,    rACT needed
//0x0e 过载  ,    rACT needed
#define D_ROBOTIQ_gFLT   0x0F
//
#define D_ROBOTIQ_kRLT   0xF0
///==byte3  写下去的pos值
#define D_ROBOTIQ_gPR   0xFF
///==byte4  真实编码器值  0xff=max
#define D_ROBOTIQ_gPO   0xFF
///==byte5  电流值   0xff=max
#define D_ROBOTIQ_gCU   0xFF


CommRobotiq ctlRobotiq;

CommRobotiq::CommRobotiq(QObject *parent) :
    QObject(parent)
{

    versionInfo.componentName="CommRobotiq";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";

    for(int i=0;i<D_REG_MAX;i++){
        r_reg[i]=0;
        w_reg[i]=0;
    }
    isCheckObj=false;

#if D_USE_QT_SERIAL
    connect(&m_reader, SIGNAL(readyRead()), this, SLOT( readyReadSlot()) );
#else
    mb=NULL;
#endif

    connect( &timer, SIGNAL(timeout()), this, SLOT(checkStatus()) );

}

int CommRobotiq::readVersion(std::vector <ComponentVersion> &componentVersionList)
{
    componentVersionList.push_back(versionInfo);
    return 1;
}

void CommRobotiq::checkStatus( )
{//定时检测
    if( mb ){
        //        r_reg[0]= 0x0001;
        if( qtlock.tryLock() )
        {
            int ret = modbus_read_registers( mb,   0x07D0 ,  5,  r_reg );

            if(  isCheckObj ){
                static int m_status=0x0;
                quint8 status=(r_reg[0]>>14) & 0xFF;
                if( status==0x02 ) // D_ROBOTIQ_gOBJ
                {
                    if( m_status != status ){
                        m_status = status;
                        qDebug()<<"hei, i get it !  pos="<<(r_reg[2]>>8) << status <<(r_reg[0]>>8);
                        isCheckObj=false;
                        emit  signal_found_obj( (r_reg[2]>>8) );
                    }
                }else{
                    //   qDebug()<< status <<(r_reg[0]>>8);
                    if( m_status==0x03 ){
                        qDebug()<<"sorry, i'm' not dound anything !  pos="<<(r_reg[2]>>8) << status <<(r_reg[0]>>8);
                        isCheckObj=false;
                        emit signal_not_found();
                    }
                    m_status=0;
                }
            }
            qtlock.unlock();
        }
    }
}

int CommRobotiq::connectRobotiq( )
{

#if D_USE_QT_SERIAL
    QSerialPortInfo info;
    foreach (  info, QSerialPortInfo::availablePorts() )
    {
        qDebug() << "Name : " << info.portName();
        qDebug() << "Description : " << info.description();
        qDebug() << "serialNumber: " << info.serialNumber();
        if( info.portName() == "ttyUSB0" )
        {
            com_info = info;

            qDebug()<<"find COM";
            break;
        }
    }

    m_reader.setPort(com_info);
    if(m_reader.open(QIODevice::ReadWrite))
    {
        qDebug() << "m_reader.open(QIODevice::ReadWrite)";
        m_reader.setBaudRate(QSerialPort::Baud115200);
        m_reader.setParity(QSerialPort::NoParity);// N
        m_reader.setDataBits(QSerialPort::Data8);// 8
        m_reader.setStopBits(QSerialPort::OneStop);// 1
        m_reader.setFlowControl(QSerialPort::NoFlowControl);

        m_reader.clearError();
        m_reader.clear();
        message( " connect ok !" );
    }else{
        message( " connect err !" );
    }

#else
    //QString ip = ui->lineEdit->text();
    if( !mb ){
        int ret =0;
        mb =  modbus_new_rtu( "/dev/ttyUSB0", 115200, 'N' , 8,   1 );
        modbus_connect ( mb ) ;
        modbus_set_slave(mb, 9);//set slave address

        //  clearError();
        acitved();
        ret = modbus_read_registers( mb,   0x07D0 ,   3,  r_reg );
        qDebug(  "active=== 0x%x  0x%x  0x%x", r_reg[0], r_reg[1] ,r_reg[2 ]  );
        if( (r_reg[0] >>8)& D_ROBOTIQ_gSTA == D_ROBOTIQ_gSTA ){
            message ("active ok !" );
        }else{
            message ("active failed   !" );
        }
    }

    if(  mb ){
        message( " connect ok !" );
        if( !timer.isActive() )
            timer.start( 10 );
        return 1;
    }else{
        message( " connect err !" );
        return 0;
    }
#endif
}


int CommRobotiq::disconnectRobotiq( )
{
    timer.stop();
#if D_USE_QT_SERIAL
    m_serialport.close();
#else
    if( mb ){
        modbus_close( mb );
        modbus_free( mb );
        mb=NULL;
    }
#endif
}

int CommRobotiq::acitved()
{
    if(  mb ){
        qtlock.lock();

#if 0
        clearError();
        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        w_reg[0] |= (D_ROBOTIQ_rGTO<<8) ;
        w_reg[1]=0;
        w_reg[2]=0;
        qDebug()<<"w"<< w_reg[0];
        int ret = modbus_write_registers( mb,   0x03E8 ,  3,  &(w_reg[0])  );
#else

        w_reg[1]=0;
        w_reg[2]=0;

        w_reg[0] &= 0x00FF;
        int ret = modbus_write_registers( mb,   0x03E8 ,  3,  &(w_reg[0])  );

        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        w_reg[0] |= (D_ROBOTIQ_rGTO<<8) ;

        qDebug()<<"w"<< w_reg[0];
        ret = modbus_write_registers( mb,   0x03E8 ,  3,  &(w_reg[0])  );
#endif

        qtlock.unlock();
        return ret;
    }
    return 0;
}

int CommRobotiq::readStatus( int& state )
{
    if(mb){
        qtlock.lock();
        int ret = modbus_read_registers( mb,   0x07D0 ,  3,  r_reg );
        qtlock.unlock();
    }
    return 1;
}

void CommRobotiq::clearError()
{
    if(  mb ){
        qtlock.lock();
        w_reg[0] &= 0x00FF;
        int ret = modbus_write_register( mb,   0x03E8 ,   (w_reg[0])  );

        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        ret = modbus_write_register( mb,   0x03E8 ,   (w_reg[0])  );
        qtlock.unlock();
    }
}

void CommRobotiq::setPos( short pos )
{
    if(  mb ){
        qtlock.lock();
        w_reg[1] &= 0xFF00;
        w_reg[1] |=  ((pos)&0x00FF);
        int ret = modbus_write_register( mb,   0x03E8+1 ,   w_reg[1]  );
        qtlock.unlock();
    }
}

void CommRobotiq::setSpeed( short sp )
{
    if(  mb ){
        qtlock.lock();
        w_reg[2] &= 0x00FF;
        w_reg[2] |= ((sp<<8)&0xFF00);
        int ret = modbus_write_register( mb,   0x03E8+2 ,   w_reg[2]  );
        qtlock.unlock();
    }
}

void CommRobotiq::setTorque( short tq )
{
    if(  mb ){
        qtlock.lock();
        w_reg[2] &= 0xFF00;
        w_reg[2] |=  ((tq)&0x00FF);
        int ret = modbus_write_register( mb,   0x03E8+2 ,   w_reg[2]  );
        qtlock.unlock();
    }
}

void CommRobotiq::stop()
{
    if(  mb ){
        qtlock.lock();
#if 1
        w_reg[0] &= 0x00FF;
        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        w_reg[1]=0;
        w_reg[2]=0;
        qDebug()<<"ws"<< w_reg[0];
        int ret = modbus_write_registers( mb,   0x03E8 ,  3,  &(w_reg[0])  );
#else
        w_reg[0] &= 0x00FF;
        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        qDebug()<<"ws"<< w_reg[0];
        int ret = modbus_write_register( mb,   0x03E8 ,   (w_reg[0])  );
#endif
        qtlock.unlock();
    }
}

void CommRobotiq::move( short pos, short sp, short  tq )
{
    if(  mb ){
        qtlock.lock();
        w_reg[0] &= 0x00FF;
        w_reg[0] |= (D_ROBOTIQ_rACT<<8);
        w_reg[0] |= (D_ROBOTIQ_rGTO<<8) ;

        w_reg[1] &= 0xFF00;
        w_reg[1] |=  ((pos)&0x00FF);

        w_reg[2] &= 0x00FF;
        w_reg[2] |= ((sp<<8)&0xFF00);

        w_reg[2] &= 0xFF00;
        w_reg[2] |=  ((tq)&0x00FF);
        int ret = modbus_write_registers( mb,   0x03E8 , 3,  &(w_reg[0] ) );
        qtlock.unlock();

        qDebug()<< "move ====" << pos << sp << tq;
    }
}

void CommRobotiq::checkObj()
{
    move( 0xff , 0x0f , 0x0f );
    isCheckObj=true;
}
